/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* BB Slammer Wrapper is an implmentation of B&B localizer that is inteded to be used as 
* standalone "ML-SLAM" algorithm. 
*
* - It uses BetBdtLocalizer to build a local map, against which it matches the given scans.
* - It maintains the pose from the start
* - It maintains a ring buffer, which includes previous scans and measurements
* - The ring buffer can be requested as EnvironmentMeasurement to get data for global (map based) localization
* @author Jari Saarinen (Jari.Saarinen@tkk.fi)
*/
#ifndef BB_SLAMMER_WRAPPER_H_
#define BB_SLAMMER_WRAPPER_H_

#include <stdio.h>
#include <stdlib.h>
#include "owndebug.h"
#include "BetBdtLocalizer.h"
#include "settings.hpp"

class BBSlammerWrapper{
		public:
				BBSlammerWrapper(); ///Default constructor
				
				BBSlammerWrapper(float pMapResolution, float pMapSize,
												 float pSearchX, float pSearchY, float pSearchA,
												 float pFdist, float pMinSpan, float pTh,
												 float pOdoCorrectDistance, float pOdoCorrectHeading,
												 float pMapUpdateDistance, float pMapUpdateHeading, float pMaxScannerRange, float pDistToEdge);
				
				BBSlammerWrapper(gim::CSettings &settings);								 
												 
				CBetBdtLocalizer *slammer;
				
				Grid::pose reference_pose; ///This is the origin of the current local map
				Grid::pose pose_now; ///This is the most recent updated pose
				
				std::vector<Grid::pose> all_poses; ///All poses stored here
				
				/**
				* Run update using latest measurement and pose
				*/
				bool updateSimple(Grid::TScan &scan, Grid::pose pos);
				
				/**
				* Sets initial pose from outside ... call before starting the estimation
				* Note that the SLAMMER operates in the laser frame
				*/
				void setInitialPose(float x, float y, float a);
				
		private:
			///Map variables
			float paramMapResolution;
			float paramMapSize;
			float paramToEdge;
			///Search Volume
			float paramSearchX; ///< Search area (X-coordinate)	
			float paramSearchY; ///< Search area (Y-coordnate)
			float paramSearchA; ///< Search area (Heading)
			///Tuning parameters
			float paramFdist; /// diastance that defines the Heading "span" 
			float paramMinSpan; /// < the minimum resolution of search
			float paramTh; /// The threshold for maximum distance that effects the weight 
			///Update parameters
			float paramOdoCorrectDistance; ///The distance between odometry measurements that will be corrected
			float paramOdoCorrectHeading;  ///The change in heading that will be corrected 
			float paramMapUpdateDistance;  ///The distance that is used for map updates
			float paramMapUpdateHeading;   ///And the same for heading
			
			int paramCheckMeasurement;    ///If 1, the algorithm will check if the measurement should be put into the map (based on parameters below)
			float paramSameValueRatio;		/// The measurement will be put into map if the number of "occupied" cells is less than the ratio
			float paramNewValueRatio;					/// The measurement will be put into map if the number of unknown cells is higher than this threshold
			///Measurement
			float paramMaxScannerRange;

			int paramUseInitialPoseFromLogfile;
			
			int paramSetInitialPoseTo;
			float paramInitPoseX;
			float paramInitPoseY;
			float paramInitPoseA;
			
			///Local map data
			std::vector< std::vector<float> > scans; ///All scans for previous map
			std::vector< Grid::pose > poses;    ///All poses for previous map
			std::vector<float> tmp_scan;
			
			Grid::pose init_est,diff;
			Grid::pose last,current;
			Grid::pose sinceLastOdoCorrection;
			Grid::pose sinceLastMapCorrection;
			
			void initVariables(float pMapResolution, float pMapSize,
												 float pSearchX, float pSearchY, float pSearchA,
												 float pFdist, float pMinSpan, float pTh,
												 float pOdoCorrectDistance, float pOdoCorrectHeading,
												 float pMapUpdateDistance, float pMapUpdateHeading, float pMaxScannerRange,
												 float pDistToEdge);
		
												 
			bool updateMeasurement(Grid::TScan &scan, Grid::pose pos, CMatchStorage matches, 
																										bool usePreviousLocalMap);
};


#endif
